/*************************************************************
*
* M2D_MATR.C
*
* 2D (3x3) matrix operations
*************************************************************/
#include <stdafx.h>

//#include <ap_comm.h>
#include <m3d\m3d.h>

// global variables
m2dMATR m2dMatrIdentity;

//
// Static variables
//

/********************************************************************
* m3dInit2DMatr ()
********************************************************************/
BOOL m3dInit2DMatr (void)
{
   m2dMatrIdentity.Identity();
   return TRUE;
}
/********************************************************************
* m3dTerm2DMatr ()
********************************************************************/
void m3dTerm2DMatr (void)
{
   return;
}

/*************************************************************
* m2dMATR related functions
*************************************************************/
/*************************************************************
* m2dAddMatr()
* 
* Add 2D matrices: c = a + b
*
* IN:  a, b - 2D matrices
* OUT: c    - 2D matrix
*************************************************************/
void m2dAddMatr (m2dMATR *a, m2dMATR *b, m2dMATR *c)
{
   int i;

   // c[row][col] = a[row][col] + b[row][col]
   // loop is the linear form of double-loop
   for (i = 0; i < 9; i++)  {
      m2dMatrElement(c,i) = m2dMatrElement(a,i) + m2dMatrElement(b,i);
   }
   return;
}

/*************************************************************
* m2dSubtractMatr()
* 
* Subtract 2D matrices: c = a - b
*
* IN:  a, b - 2D matrices
* OUT: c    - 2D matrix
*************************************************************/
void m2dSubtractMatr (m2dMATR *a, m2dMATR *b, m2dMATR *c)
{
   int i;

   // c[row][col] = a[row][col] - b[row][col]
   // loop is the linear form of double-loop
   for (i = 0; i < 9; i++)  {
      m2dMatrElement(c,i) = m2dMatrElement(a,i) - m2dMatrElement(b,i);
   }
   return;
}
/*************************************************************
* m2dMultiplyMatr()
* 
* Multiply 2D matrices: c = a * b
*
* IN:  a, b - 2D matrices
* OUT: c    - 2D matrix
*************************************************************/
void m2dMultiplyMatr (m2dMATR *a, m2dMATR *b, m2dMATR *c)
{
   int       i, row, col;
   m2dMATR   tmp, *_c;

   _c = c;
   if (a == c || b == c) {
      _c  = &tmp;
   }

   for (row = 0; row < 3; row++)  {
      for (col = 0; col < 3; col++)  {
         m2dMatrElementRC(_c,row,col) = 0.f;
         for (i = 0; i < 3; i++)  {
            m2dMatrElementRC(_c,row,col) += m2dMatrElementRC(a,row,i) * m2dMatrElementRC(b,i,col);
         }
      }
   }
   if (a == c || b == c) {
      *c = tmp;
   }
   return;
}

/*************************************************************
* m2dMATR Class methods
*************************************************************/
/*************************************************************
* m2dMATR::Identity()
*
* Make 2D Identity matrix
*
*************************************************************/
void m2dMATR::Identity (void)
{
   int i;

   memset(this, 0, sizeof(m2dMATR));
   for (i = 0; i < 3; i++ )  {
      ElementRC(i,i) = 1.f;
   }
   return;
}

/*************************************************************
* m2dMATR::Scale ()
* 
* Scale 2D matrix: a *= s
*
* IN:  a   - 2D matrix
*      s   - scale factor
* OUT: a   - scaled 2D matrix
*************************************************************/
void m2dMATR::Scale (float sx, float sy, int oper)
{
   int  i;
#ifndef _RELEASE
   int  j;
#endif

   ASSERT(oper == M3D_REPLACE || oper == M3D_PRE || oper == M3D_POST);
   switch (oper) {
      case M3D_PRE:
         for (i=0; i<2; i++) {
            ElementRC(0, i) *= sx;
            ElementRC(1, i) *= sy;
         }
         break;
      case M3D_POST:
         for (i=0; i<3; i++) {
            ElementRC(i, 0) *= sx;
            ElementRC(i, 1) *= sy;
         }
         break;
      case M3D_REPLACE:
         // this operation can be done only for matrices that do not contain
         // any other than scale information!!!
         // check this
#ifndef _RELEASE
         for (i=0; i<3; i++) {
            for (j=0; j<3; j++) {
               if (i != j) {
                  ASSERT(m3dIsZero_A(ElementRC(i, j), M3D_EPSILON));
               }
            }
         }
#endif
         ElementRC(0, 0) = sx;
         ElementRC(1, 1) = sy;
         break;
   }
   return;
}
/*************************************************************
* m2dMATR::Translate ()
* 
* IN:  pM   - 2D matrix
*      x, y - translation offsets
*      oper - operation (M3D_PRE, M3D_POST, M3D_REPLACE)
* OUT: pM   - translated matrix
*************************************************************/
void m2dMATR::Translate (float x, float y, int oper)
{
   switch (oper) {
      case M3D_REPLACE:
         ElementRC(2, 0) = x;
         ElementRC(2, 1) = y;
         break;
      case M3D_PRE:
         ElementRC(2, 0) += ElementRC(0, 0)*x + ElementRC(1, 0)*y;
         ElementRC(2, 1) += ElementRC(0, 1)*x + ElementRC(1, 1)*y;
         break;
      case M3D_POST:
         ElementRC(2, 0) += x;
         ElementRC(2, 1) += y;
         break;
      default:
         ASSERT(FALSE);
   }
   return;
}

/*************************************************************
* m2dMATR::Transform ()
*
* General 2D matrix transformation
*
* IN:  pM      - matrix to be transformed
*      pMTrans - transformation matrix
*      oper    - operation (M3D_PRE, M3D_POST, M3D_REPLACE)
* OUT: pM      - transformed matrix
*************************************************************/
void m2dMATR::Transform (m2dMATR *pMTrans, int oper)
{
   m2dMATR  matrTmp;

   ASSERT(oper == M3D_REPLACE || oper == M3D_PRE || oper == M3D_POST);

   if (oper == M3D_REPLACE)  {
      *this = *pMTrans;
   } else if (oper == M3D_POST)  {
      m2dMultiplyMatr(this, pMTrans, &matrTmp);
      *this = matrTmp;
   } else { // oper == M3D_PRE
      m2dMultiplyMatr(pMTrans, this, &matrTmp);
      *this = matrTmp;
   }

   return;
}

/*************************************************************
* m2dMATR::Rotate()
*
* 2D matrix rotation
*
* IN:  pM      - 2D matrix
*      angle   - angle rotation
*      oper    - operation (M3D_PRE, M3D_POST, M3D_REPLACE)
* OUT: pM      - rotated matrix
*************************************************************/
void m2dMATR::Rotate (float angle, int oper)
{
   float  cosA, sinA;

   // REPLACE is not valid for rotations
   ASSERT(oper == M3D_PRE || oper == M3D_POST);

   angle = m3dDeg2Rad(angle);

   cosA = m3dCos(angle);
   sinA = m3dSin(angle);

   m2dMATR  _m2dMatrRot;

   // construct rotX transformation matrix
   _m2dMatrRot.Identity();
   _m2dMatrRot.ElementRC(0, 0) =  cosA;
   _m2dMatrRot.ElementRC(0, 1) =  sinA;
   _m2dMatrRot.ElementRC(1, 0) = -sinA;
   _m2dMatrRot.ElementRC(1, 1) =  cosA;

   Transform(&_m2dMatrRot, oper);

   return;
}

/**********************************************************************
* m2dMATR::MakeRot()
*
* Make rotation matrix. Rotation around (0, 0) by 'phi' degrees
*
* IN :  phi - angle of rotation (in degrees)
* OUT:  pM  - matrix
**********************************************************************/
void m2dMATR::MakeRot (float phi)
{
   memset(this, 0, sizeof(m2dMATR));

   phi = m3dDeg2Rad(phi);
   ElementRC(0, 0) = (float) m3dCos(phi);
   ElementRC(0, 1) = (float)-m3dSin(phi);
   ElementRC(1, 0) = (float) m3dSin(phi);
   ElementRC(1, 1) = (float) m3dCos(phi);

   return;
}


/*************************************************************
* m2dMATR::GetOrigin()
*
* Get matrix origin
* IN : pMat - matrix (specifying some LCS)
* OUT: org  - origin of the matrix
*************************************************************/
void m2dMATR::GetOrigin (m2dV *org)
{
   org->x = ElementRC(2, 0);
   org->y = ElementRC(2, 1);

   return;
}
/********************************************************
* m2dMATR::MakeLSegLSeg ()
*   Trasform one line segment into another
* IN:  lB1, lE1 - source line segment
*      lB2, lE2 - destination line segment
* OUT: the transformation matrix transforming vertices of 
* one line segment into another
*********************************************************/
void m2dMATR::MakeLSegLSeg (m2dV *lB1, m2dV *lE1, m2dV *lB2, m2dV *lE2)
{
   m2dV   dir1, dir2;
   float  scale;

   m2dSubtractVector(lE1, lB1, &dir1);
   m2dSubtractVector(lE2, lB2, &dir2);

   float dist = m2dDist(lB1, lE1);
   if (!m3dIsZero(dist)) {
      scale = m2dDist(lB2, lE2) / dist;
   } else {
      scale = 1.f;
   }
   
   Identity();
   Translate(-lB1->x, -lB1->y, M3D_POST);
   Rotate(m2dAngleVector(&dir2, &dir1), M3D_POST);
   Scale(scale, scale, M3D_POST);
   Translate(lB2->x, lB2->y, M3D_POST);

#ifdef _DEBUG
   m2dV  _lB, _lE;

   TransformPoint(lB1, &_lB);
   TransformPoint(lE1, &_lE);

   ASSERT(m2dIsEqualPoint_A(lB2, &_lB, 100*M3D_EPSILON));
   ASSERT(m2dIsEqualPoint_A(lE2, &_lE, 100*M3D_EPSILON));
#endif

#ifndef _RETAIL
   for (int i=0; i < 3; ++i) {
      STRONG_ASSERT(_m3dCheckValid((m3dV *)m[i]));
   }
#endif
   return;
}

/********************************************************
* m2dMATR::MakePolyPoly ()
* IN:  pPolyFrom
*      pPolyTo    - two polygons
* OUT: pMat - matrix of the affine transformation
*      transforming 3 first vertices of pPolyFrom into 
*      3 first vertices of pPolyTo
* RET: FALSE if error occures (pPolyFrom is degenerate)
*********************************************************/
BOOL m2dMATR::MakePolyPoly (m2dPOLY *pPolyFrom, m2dPOLY *pPolyTo)
{
   m2dMATR matrResult;
   matrResult.ElementRC(0,2) = 0.f;
   matrResult.ElementRC(1,2) = 0.f;
   matrResult.ElementRC(2,2) = 1.f;

   m3dMATR matrBlock;
   for (int i=0; i<3; i++) {
      matrBlock.ElementRC(i,3) = 0.f;
      matrBlock.ElementRC(3,i) = 0.f;
      matrBlock.ElementRC(2,i) = 1.f;
      matrBlock.ElementRC(0,i) = m3dPolyVertexX(pPolyFrom,i);
      matrBlock.ElementRC(1,i) = m3dPolyVertexY(pPolyFrom,i);
   }
   matrBlock.ElementRC(3,3) = 1.f;

   m3dMATR matrBlock_inverted;
   BOOL rc = matrBlock.Invert(&matrBlock_inverted);
   if (!rc) {
      return FALSE;
   }

   m3dV vRight;
   vRight.x = m3dPolyVertexX(pPolyTo,0);
   vRight.y = m3dPolyVertexX(pPolyTo,1);
   vRight.z = m3dPolyVertexX(pPolyTo,2);
   
   m3dV vTmp3D;
   matrBlock_inverted.TransformVector(&vRight, &vTmp3D);

   matrResult.ElementRC(0,0) = vTmp3D.x;
   matrResult.ElementRC(1,0) = vTmp3D.y;
   matrResult.ElementRC(2,0) = vTmp3D.z;

   vRight.x = m3dPolyVertexY(pPolyTo,0);
   vRight.y = m3dPolyVertexY(pPolyTo,1);
   vRight.z = m3dPolyVertexY(pPolyTo,2);
   matrBlock_inverted.TransformVector(&vRight, &vTmp3D);

   matrResult.ElementRC(0,1) = vTmp3D.x;
   matrResult.ElementRC(1,1) = vTmp3D.y;
   matrResult.ElementRC(2,1) = vTmp3D.z;

#ifdef _DEBUG
   m2dPOLY polyTmp;
   matrResult.TransformPoly(pPolyFrom, &polyTmp);
   for (int i=0; i<3; i++) {
      ASSERT(m3dIsZero_A(m2dDist(m3dPolyVertex(&polyTmp,i), m3dPolyVertex(pPolyTo,i)),0.001f));
   }
#endif // _DEBUG
   *this = matrResult;
   return TRUE;
}
/*************************************************************
* m2dMATR::CalcDet ()
*
* IN : pMatr - matrix
* RET: matrix determinant
* Calculate determinant of 3*3 matrix
*************************************************************/
float m2dMATR::CalcDet (void)
{
   float det  = ElementRC(0, 0)*ElementRC(1, 1)*ElementRC(2, 2);
   det       += ElementRC(0, 2)*ElementRC(1, 0)*ElementRC(2, 1);
   det       += ElementRC(0, 1)*ElementRC(1, 2)*ElementRC(2, 0);
   det       -= ElementRC(0, 2)*ElementRC(1, 1)*ElementRC(2, 0);
   det       -= ElementRC(0, 1)*ElementRC(2, 2)*ElementRC(1, 0);
   det       -= ElementRC(0, 0)*ElementRC(1, 2)*ElementRC(2, 1);

   return det;
}

/*************************************************************
* m2dMATR::Invert ()
* 
* IN:  srcMatr  - source matrix (to be inverted)
* OUT: destMatr - inverted matrix
* RET: TRUE  - managed to invert matrix
*      FALSE - could not manage to invert matrix
*************************************************************/
BOOL m2dMATR::Invert (m2dMATR *destMatr)
{
   float  det, oodet;
   
   ASSERT(ElementRC(0, 2) == 0);
   ASSERT(ElementRC(1, 2) == 0);
   
   det = ElementRC(0,0) * ElementRC(1,1) - ElementRC(0,1) * ElementRC(1,0);
   if (m3dIsZero(det)) {
      return FALSE;
   }
   oodet = 1.f/det;
   destMatr->ElementRC(0,0) =   ElementRC(1,1) * oodet;
   destMatr->ElementRC(1,1) =   ElementRC(0,0) * oodet;
   destMatr->ElementRC(0,1) = - ElementRC(0,1) * oodet;
   destMatr->ElementRC(1,0) = - ElementRC(1,0) * oodet;

   return TRUE;
}

//
// End-of-file M2D_MATR.CPP
//
